/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DoubleSolenoid;
//import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Timer;

import file.Configurable;
import file.Configuration;
import file.DataRecorder;

/**
 *
 * @author Mentors  Carl Burger - Team 708
 */
public class Elevator implements Configurable {

    // Sensors
    private  AnalogChannel    elevatorPot;      // potentiometer for elevator position

    // Actuators
    private  DoubleSolenoid   brakePiston;
//    private  Jaguar           elevatorMotor;
    private  Victor           elevatorMotor;

    private  Wrist            wrist;

    private  Configuration    config;
    private  DataRecorder     dataRecorder;
    private  MessageCenter    messageCenter;

    private  int              previousPos  = 0;           // for message center display
    private  double           previousElevatorOutput = -2; //make sure value is output

    private  Timer            timer;
    private  double           elevatorTimeLimit = 5; // max secs to reach setpoint

    private  PIDController708 elevatorControl;

    private  double minimumInput     = 20;    // potentiometer minimum average voltage; ~0.18
    private  double maximumInput     = 708;    // potentiometer maximum average voltage; ~3.64
    private  double tolerance        = 1.0;    // 1% of (max - min)
    private  double minimumOutput    = -1.0;   // motor minimum speed
    private  double maximumOutput    = 1.0;    // motor maximum speed
    private  double proportionalUp   = 0.25;   // needs to be tuned
    private  double integralUp       = 0.005;  // needs to be tuned
    private  double differentialUp   = 0.0;    // needs to be tuned
    private  double proportionalDown = 0.01;   // needs to be tuned
    private  double integralDown     = 0.0001; // needs to be tuned
    private  double differentialDown = 0.04;   // needs to be tuned

//    private  double potAdjustment          = 70;
    //offset 11/12/11: 64 @ initial pos, or +30
    private  double initialPos             = 34;  // ~0.17; elevator position at start of match
    private  double drivePos               = 34;  // ~0.17;
    private  double feederPos              = 233; // ~1.2v
    private  double floorPos               = 49;  // ~0.25v
    private  double carabinerEngagementPos = 104;
    private  double minibotReleasePos      = 297;
    private  double bottomPegPosFromFloor  = 49;  // ~0.25v
    private  double middlePegPosFromFloor  = 334; // ~1.72v
    private  double topPegPosFromFloor     = 643; // ~3.31v
    private  double bottomPegPosFromFeeder = 34;  // ~0.18v  (upside down is 317/1.63v)
    private  double middlePegPosFromFeeder = 255; // ~1.31v
    private  double topPegPosFromFeeder    = 568; // ~2.92v
    private  double pegOffset              = 89;  // extra height for center post; 0.5v
    private  double releaseOffset          = 100; //distance to move elevator after release

    private  double manualSpeedGain = 2.0; // see constructor for use
    private  double manualSpeed;           // computed using  gain * tolerance
    
    private  double speedLimitCoefficient = 1; //needs to be tuned
    private  double startLimitingSpeedAt  = 2; //needs to be tuned
    private  double elevatorDeadZone      = 0.2; //absolute value of range around 0 in which JS input is ignored

    private  double lastManualMovement = 0.0; // last input from joystick (0 if in dead zone)

    private static Elevator m_instance = null;

    /**
     *  Get an instance of the Elevator
     */
    public static Elevator getInstance() {
        if (m_instance == null) {
            m_instance = new Elevator();
        }
        return m_instance;
    }


    private Elevator() {

        elevatorPot   = new AnalogChannel(LogomotionRobot.kElevator);

        brakePiston   = new DoubleSolenoid(LogomotionRobot.kBrakePistonForward,
                                           LogomotionRobot.kBrakePistonReverse);
        brakePiston.set(DoubleSolenoid.Value.kForward);

//        elevatorMotor = new Jaguar(LogomotionRobot.kElevatorController);
        elevatorMotor = new Victor(LogomotionRobot.kElevatorController);
//        elevatorMotor.setExpiration(5);
//        elevatorMotor.setSafetyEnabled(true);

        wrist         = Wrist.getInstance();

        timer         = new Timer();

        // initialize the PIDController which controls elevator movement

        elevatorControl = new PIDController708(proportionalUp, integralUp, differentialUp,
                                               elevatorPot, elevatorMotor);
        elevatorControl.setInputRange(minimumInput, maximumInput);
        elevatorControl.setTolerance(tolerance);
        elevatorControl.setOutputRange(minimumOutput, maximumOutput);

        // ensure the manual speed delta exceeds the tolerance so that the
        // PIDController is guaranteed to move the elevator
        manualSpeed  = manualSpeedGain * ((maximumInput - minimumInput) * tolerance) / 100;

        dataRecorder    = DataRecorder.getInstance();

        messageCenter   = MessageCenter.getInstance();

        config = Configuration.getInstance();
        config.register("Elevator", this);
    }

    public void monitorMovement() {

        // if the elevator is moving, check if it reached the commanded position

//        System.out.println("monitoringMovement " + " " + elevatorControl.onTarget() +
//                           " " + elevatorControl.isEnable() + " " + timer.get());

        if (elevatorControl.isEnable()) {
            if (timer.get() > elevatorTimeLimit)
                System.out.println("Elevator PID controller timeout");
            if (elevatorControl.onTarget() || timer.get() > elevatorTimeLimit) {
                
                // stop running the PIDController. Sets output to 0 before stopping
                elevatorControl.reset();

                brakePiston.set(DoubleSolenoid.Value.kForward);   // set the brake
                System.out.println("Stopping elevator at " + elevatorPot.getAverageValue() + " " + timer.get());

                timer.stop();
            }

//            System.out.println("monitoringMovement " + " " + elevatorControl.onTarget() +
//                               " " + elevatorControl.getError() + " " + timer.get());
        }
    }

    private void moveElevatorTo(double pos) {

        // position the elevator to commanded position
        elevatorControl.setSetpoint(pos);

        System.out.println("moveElevatorTo " + " " + elevatorControl.onTarget() + " " + elevatorControl.isEnable());

        if (elevatorControl.onTarget() == false) {
            if (elevatorControl.isEnable() == false) {

                brakePiston.set(DoubleSolenoid.Value.kReverse); // release the brake
                System.out.println("Moving elevator to " + pos);

                if (pos > elevatorPot.getAverageValue()) {

                    elevatorControl.setPID(proportionalUp, integralUp, differentialUp);
                    System.out.println("PID Up  " + proportionalUp + "  " + integralUp + "  " + differentialUp);
                    
                }  else {

                    elevatorControl.setPID(proportionalDown, integralDown, differentialDown);
                    System.out.println("PID Down  " + proportionalDown + "  " + integralDown + "  " + differentialDown);

                }
                
                elevatorControl.reset();  // clear previous errors
                elevatorControl.enable(); // start calculating PIDOutput values

                timer.reset();
                timer.start();
            }
        }
    }

    public void moveToInitialStartPosition() {

        // position the elevator for the start of a match
        moveElevatorTo(initialPos);
    }

    public void moveToDrivePosition() {

        // position the elevator for driving
        moveElevatorTo(drivePos);
    }

    public void moveToFeeder() {

        // position the elevator to receive a shape from the feeder
        moveElevatorTo(feederPos);
    }

    public void moveToFloor() {

        // position the elevator to pick up a shape from the floor
        moveElevatorTo(floorPos);
    }

    public void moveToCarabinerEngagement()
    {
        moveElevatorTo(carabinerEngagementPos);
    }

    public void moveToMinibotReleasePoint()
    {
        moveElevatorTo(minibotReleasePos);
    }

    public void moveToTopPeg(boolean offset) {

        double pos;
        boolean fromFloor = wrist.fromFloor();
        
        pos = fromFloor ? topPegPosFromFloor : topPegPosFromFeeder;

        // position the elevator to hang a shape on the top peg of the grid
        if (offset) {
            pos += pegOffset;
        }
        moveElevatorTo(pos);
    }

    public void moveToMiddlePeg(boolean offset) {

        double pos;
        boolean fromFloor = wrist.fromFloor();

        pos = fromFloor ? middlePegPosFromFloor : middlePegPosFromFeeder;

        // position the elevator to hang a shape on the middle peg of the grid
        if (offset) {
            pos += pegOffset;
        }
        moveElevatorTo(pos);
    }

    /*
     * Temporarily altered to move elevator to a set # of pts
     * below current position
     */
    public void moveToBottomPeg(boolean offset) {

        double pos;
        boolean fromFloor = wrist.fromFloor();

//        pos = fromFloor ? bottomPegPosFromFloor : bottomPegPosFromFeeder;
        
        pos = elevatorPot.getAverageValue() - 70;
        if (pos < minimumInput) {
            pos = minimumInput;
        }

        // position the elevator to hang a shape on the bottom peg of the grid
//        if (offset) {
//            pos += pegOffset;
//        }
        moveElevatorTo(pos);
    }

    public void moveAfterRelease(){
        double currentPos = elevatorPot.getAverageValue();

        moveElevatorTo(currentPos - releaseOffset);

    }

    public void move(double jsInput) {

        //do not use joystick if PID control is running
        if (elevatorControl.isEnable() == false) {

            // allow the elevator to be manually moved using a joystick for debugging

            double input = jsInput;

            if ((jsInput > -elevatorDeadZone) && (jsInput < elevatorDeadZone)) {
                input = 0.0; // ignore possible joystick jitter around 0
            }

            // if raising the elevator and the elevator is fully extended, then stop
            if ((jsInput > 0) && (elevatorPot.getAverageValue() >= maximumInput)) {
                input = 0;
            }

            // if lowerng the elevator and the elevator is fully retracted, then stop
            if ((jsInput < 0) && (elevatorPot.getAverageValue() <= minimumInput)) {
                input = 0;
            }

            // check if starting, stopping, or changing speed
            if (input != lastManualMovement) {

                if (input == 0) {
                    // previously moving, but now commanded to stop
                    System.out.println("Set brake");
                    brakePiston.set(DoubleSolenoid.Value.kForward); // set the brake

                } else {
                    if (lastManualMovement == 0) {
                        // previously stopped, but now commanded to move
                        System.out.println("Release brake");
                        brakePiston.set(DoubleSolenoid.Value.kReverse); // release the brake
                    }
                    else {
                        // previously moving, but now commanded to move at a different speed
                        // brake is already released, so nothing to do
                    }
                }

//                System.out.println("Elevator move jsInput " + jsInput +
//                        " voltage: " + messageCenter.formatDouble(elevatorPot.getVoltage(), 6) +
//                        " pot_pidGet: " + messageCenter.formatDouble(elevatorPot.pidGet(), 6));

                // check if lowering the elevator.  Start slowly if near top.
                if (jsInput < -0.3) {
                    if (elevatorPot.getVoltage() > 3.0) {
                        input = -0.3;  // limit the speed going down
                    }
                }

                lastManualMovement = input;
            }
            elevatorMotor.set(input);

        }
    }

    public void lower() {

        double newPos;

        // allow the elevator to be manually lowered by the operator

        newPos = elevatorPot.pidGet() - manualSpeed;
        if (newPos >= minimumInput) {
            moveElevatorTo(newPos);
        }
    }

    public void raise() {

        double newPos;

        // allow the elevator to be manually raised by the operator
        // an upper limit is required

        newPos = elevatorPot.pidGet() + manualSpeed;
        if (newPos <= maximumInput) {
            moveElevatorTo(newPos);
        }
    }

    public boolean atPosition() {

        return elevatorControl.onTarget();
    }

    public double getMaxSpeedCoefficient()
    {
        double position = elevatorPot.getVoltage();
        if (position < startLimitingSpeedAt)
        {
            return 1;  // no speed restriction
        }
        else
        {
            return speedLimitCoefficient / (position - (startLimitingSpeedAt - 1));
        }
    }

    /**
     * displaySensorStatus is called from monitorMovement()
     * to display the sensor value and motor output
     */
    public void outputStatus() {

        int position = elevatorPot.getAverageValue();

        if (position != previousPos) {

            messageCenter.println(MessageCenter.kSensorsScreen, MessageCenter.kLine1,
                    "Elevator: " + messageCenter.formatDouble(elevatorPot.getVoltage(), 5) +
                    "v " + position);

            previousPos = position;
        }

        if(elevatorMotor.get() != previousElevatorOutput){

            //display motor output
             messageCenter.println(MessageCenter.kSensorsScreen,
                    MessageCenter.kLine2,
                    "MotorOut: " + messageCenter.formatDouble(elevatorMotor.get(), 5));

             previousElevatorOutput = elevatorMotor.get();
        }
    }

    public void recordStatus(){
        dataRecorder.record("elevator pos",         elevatorPot.pidGet());
        dataRecorder.record("elevator pos avg value", elevatorPot.getAverageValue());
        dataRecorder.record("elevator setpoint",    elevatorControl.getSetpoint());
        dataRecorder.record("elevator error",      elevatorControl.getError());
        dataRecorder.record("P coeff",    elevatorControl.getP());
        dataRecorder.record("I coeff",    elevatorControl.getI());
        dataRecorder.record("D coeff",    elevatorControl.getD());
        dataRecorder.record("PID result", elevatorControl.get());
    }

    public void update(String name, String[] values) {

        if (name.equals("Minimum.Input")) {
            minimumInput = Double.parseDouble(values[0]);
            elevatorControl.setInputRange(minimumInput, maximumInput);

        } else if (name.equals("Maximum.Input")) {
            maximumInput = Double.parseDouble(values[0]);
            elevatorControl.setInputRange(minimumInput, maximumInput);

        } else if (name.equals("Tolerance")) {
            tolerance = Double.parseDouble(values[0]);
            elevatorControl.setTolerance(tolerance);

        } else if (name.equals("Mimimum.Output")) {
            minimumOutput = Double.parseDouble(values[0]);
            elevatorControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Maximum.Output")) {
            maximumOutput = Double.parseDouble(values[0]);
            elevatorControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Proportional.Up")) {
            proportionalUp = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalUp, integralUp, differentialUp);

        } else if (name.equals("Integral.Up")) {
            integralUp = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalUp, integralUp, differentialUp);

        } else if (name.equals("Differential.Up")) {
            differentialUp = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalUp, integralUp, differentialUp);

        } else if (name.equals("Proportional.Down")) {
            proportionalDown = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalDown, integralDown, differentialDown);

        } else if (name.equals("Integral.Down")) {
            integralDown = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalDown, integralDown, differentialDown);

        } else if (name.equals("Differential.Down")) {
            differentialDown = Double.parseDouble(values[0]);
            elevatorControl.setPID(proportionalDown, integralDown, differentialDown);

        } else if (name.equals("Initial.Pos")) {
            initialPos = Double.parseDouble(values[0]);

        } else if (name.equals("Drive.Pos")) {
            drivePos = Double.parseDouble(values[0]);

        } else if (name.equals("Feeder.Pos")) {
            feederPos = Double.parseDouble(values[0]);

        } else if (name.equals("Floor.Pos")) {
            floorPos = Double.parseDouble(values[0]);

        } else if (name.equals("Carabiner.Engagement.Pos")) {
            carabinerEngagementPos = Double.parseDouble(values[0]);

        } else if (name.equals("Minibot.Release.Pos")) {
            minibotReleasePos = Double.parseDouble(values[0]);

        } else if (name.equals("BottomPeg.Pos.From.Floor")) {
            bottomPegPosFromFloor = Double.parseDouble(values[0]);

        } else if (name.equals("MiddlePeg.Pos.From.Floor")) {
            middlePegPosFromFloor = Double.parseDouble(values[0]);

        } else if (name.equals("TopPeg.Pos.From.Floor")) {
            topPegPosFromFloor = Double.parseDouble(values[0]);

        } else if (name.equals("BottomPeg.Pos.From.Feeder")) {
            bottomPegPosFromFeeder = Double.parseDouble(values[0]);

        } else if (name.equals("MiddlePeg.Pos.From.Feeder")) {
            middlePegPosFromFeeder = Double.parseDouble(values[0]);

        } else if (name.equals("TopPeg.Pos.From.Feeder")) {
            topPegPosFromFeeder = Double.parseDouble(values[0]);

        } else if (name.equals("Peg.Offset")) {
            pegOffset = Double.parseDouble(values[0]);

        }else if(name.equals("Release.Offset")){
            releaseOffset = Double.parseDouble(values[0]);

        } else if (name.equals("Manual.Speed.Gain")) {
            manualSpeedGain = Double.parseDouble(values[0]);
            // the gain can't be less than 1, or the tolerance won't be exceeded
            if (manualSpeedGain > 1.0) {
                manualSpeed  = manualSpeedGain * ((maximumInput - minimumInput) * tolerance) / 100;
            }

        } else if (name.equals("Speed.Limit.Coefficient")) {
            speedLimitCoefficient = Double.parseDouble(values[0]);

        } else if (name.equals("Start.Limiting.Speed.At")) {
            startLimitingSpeedAt = Double.parseDouble(values[0]);

        }else if (name.equals("Time.Limit")) {
            elevatorTimeLimit = Double.parseDouble(values[0]);
        }else if (name.equals("DeadZone")){
            elevatorDeadZone = Double.parseDouble(values[0]);
        }
    }
}